#ifndef EKF_H
#define EKF_H

#include <opencv2/opencv.hpp>

using namespace cv;

class EkfSlam {
	// Map
	RNG *r;
	Mat mState, q, s;
	Mat Prr, Prm, Pmr, Pmm;
	// kalman
	Mat zbar, Z, K;

	// Motion
	Mat fv(Mat R, Mat u, Mat n);
	Mat df_dxb(Mat R, Mat u, Mat n);
	Mat df_dn(Mat R, Mat u, Mat n);

	// Direct Observation
	Mat h(Mat R, Mat Li);
	Mat dh_dxb(Mat R, Mat Li);
	Mat dh_dLi(Mat R, Mat Li);

	// Inverse Observation
	Mat g(Mat R, Mat yi);
	Mat dg_dxb(Mat R, Mat yi);
	Mat dg_dyi(Mat R, Mat yi);

	void move(Mat R, Mat u, Mat n);
	void correct(Mat yi, Mat v, int i);
	void addFeature(Mat yi, int i);

	Mat (*scan)(int);
public:
	EkfSlam(Mat (*s)(int), Mat qi, Mat si);
	~EkfSlam();
	Mat step(Mat u);
	bool getMap(Mat& map, Mat& cov);
	Mat getRobot();
};

#endif
